unit CoordinateSystem;

interface

uses Math, Mathematic, Vector;

type
  TCoordinateSystem = class
  private
    aForward: TVector;
    aSide: TVector;
    aUp: TVector;
    aPosition: TVector;
  public
    constructor Create;
    constructor CreateCopy(paCoordinateSystem: TCoordinateSystem);
    destructor Destroy;override;
    procedure Setup(paPosition, paForward: TVector);
    function LocalizeDirection(paGlobalDirection: TVector): TVector;
    function LocalizePosition(paGlobalPosition: TVector): TVector;
    function GlobalizeDirection(paLocalDirection: TVector): TVector;
    function GlobalizePosition(paLocalPosition: TVector): TVector;
    property Forward: TVector read aForward write aForward;
    property Side: TVector read aSide write aSide;
    property Up: TVector read aUp write aUp;
    property Position: TVector read aPosition write aPosition;
  end;

implementation

constructor TCoordinateSystem.Create;
begin
    inherited Create;
    aForward := TVector.Create(1.0, 0.0, 0.0);
    aSide := TVector.Create(0.0, 1.0, 0.0);
    aUp := TVector.Create(0.0, 0.0, 1.0);
    aPosition := TVector.Create(0.0, 0.0, 0.0);
end;

constructor TCoordinateSystem.CreateCopy(paCoordinateSystem: TCoordinateSystem);
begin
    inherited Create;
    aForward := TVector.CreateCopy(paCoordinateSystem.aForward);
    aSide := TVector.CreateCopy(paCoordinateSystem.aSide);
    aUp := TVector.CreateCopy(paCoordinateSystem.aUp);
    aPosition := TVector.CreateCopy(paCoordinateSystem.aPosition);
end;

destructor TCoordinateSystem.Destroy;
begin
    aForward.Free;
    aSide.Free;
    aUp.Free;
    aPosition.Free;
    inherited Destroy;
end;

procedure TCoordinateSystem.Setup(paPosition, paForward: TVector);
begin
    aPosition.MakeCopy(paPosition);
    aForward.MakeCopy(paForward);
    aForward.NormalizeSelf;
    aSide.Free;
//    aSide := aUp.CrossProduct(aForward);
    aSide := aForward.CrossProduct(aUp);
    aSide.NormalizeSelf;
end;

function TCoordinateSystem.LocalizeDirection(paGlobalDirection: TVector): TVector;
begin
    Result := TVector.Create(paGlobalDirection.DotProduct(aForward),
                             paGlobalDirection.DotProduct(aSide),
                             paGlobalDirection.DotProduct(aUp));
end;

function TCoordinateSystem.LocalizePosition(paGlobalPosition: TVector): TVector;
var Vector: TVector;
begin
    Vector := paGlobalPosition.Subtract(aPosition);
    Result := LocalizeDirection(Vector);
    Vector.Free;
end;

function TCoordinateSystem.GlobalizeDirection(paLocalDirection: TVector): TVector;
var F, S, U: TVector;
begin
    F := aForward.Multiply(paLocalDirection.X);
    S := aSide.Multiply(paLocalDirection.Y);
    U := aUp.Multiply(paLocalDirection.Z);
    Result := F.AddSelf(S).AddSelf(U);
    S.Free; U.Free;
end;

function TCoordinateSystem.GlobalizePosition(paLocalPosition: TVector): TVector;
var Vector: TVector;
begin
    Vector := GlobalizeDirection(paLocalPosition);
    Vector := Vector.AddSelf(aPosition);
    Result := Vector;
end;

end.
